/**
 ******************************************************************************
  * @file           : unitree_motor_control.c
  * @brief          : 宇树电机驱动相关文件，包含以uart驱动的函数和电机任务
  * 已验证
  ******************************************************************************
  * @attention
  * can转485模块默认ID为3，需要确认好ID，此处以默认ID为例
  * go电机和a1电机协议不同，需要分辨！！！
  * 电机id可以通过上位机读取
  * 电机驱动方式是混合驱动，可以通过单独给某几个参数设置值达到不同的驱动方式
  * 1.位置模式：设置position和kp，kw，T和W置0；
  * 2.速度模式：设置W和kw，其它参数置0(当W=0时，电机会有阻尼，但是不会转动)；
  *             (空载参考设定值：W=1,，kw=0.02)
  * 3.力矩模式：设置T，其它参数置0(当T=0时，电机明显无阻力)；
  *             (空载参考设定值：T=0.05)
  * 4.混合模式：这个模式就是设置所有参数值，让电机可以按照需要的速度和力矩运行到
  *             需要的位置；
  * 
  * 设定的值都是以电机转子转动一圈为基准，需要乘电机的减速比才能得到输出轴
  * 转动一圈的数据
  * 减速比(输出轴:转子)
  * go-m8010电机减速比：1:6.33
  * A 1 电 机 减 速 比：1:9.1 
  * 
  ******************************************************************************** 
  * 需要注意的是，在使用遥控器设置参数的时候，要先单独读取遥控器数据，再进行计算 *
  ********************************************************************************
 */

#include "unitree_motor_control.h" 


/*电机发送和接受数据的结构体*/
UNITREE_MOTOR_send UNITREE_motor_s = {0}; //发送数据结构体
UNITREE_MOTOR_recv UNITREE_motor_r = {0}; //接受数据结构体
extern remoter_t *rc; 
float Init_Pos;
float Min_Pos;
extern __IO uint32_t uwTick;

extern float MIN_POS;
extern float MAX_POS;
HAL_StatusTypeDef  unitree_motor_init(uint8_t go_motor_num,uint8_t a1_motor_num,UNITREE_MOTOR_send *p_data)
{
    for(unsigned short motor_id = 0; motor_id < go_motor_num || motor_id < a1_motor_num; motor_id++)
    {
        p_data->id=motor_id;
        p_data->K_P=0;
        p_data->K_W=go_empty_kw;
        p_data->mode=1;
        p_data->Pos=0;
        p_data->T=0;
        p_data->W=0;
        if(motor_id < go_motor_num)
            unitree_uart_Send_recv(GO_motor,&unitree_huart,p_data,&UNITREE_motor_r);
        if(motor_id < a1_motor_num)
            unitree_uart_Send_recv(A1_motor,&unitree_huart,p_data,&UNITREE_motor_r);
    }
}

/***************GO电机控制函数***************/
/**
 * @brief 发送数据
 * @param GO_motor_s 发送数据结构体
 */
int GO_send_data(UART_HandleTypeDef *huart,UNITREE_MOTOR_send *GO_motor_s,uint16_t *rxlen)
{
    GO_motor_s->hex_len = 17;
    GO_motor_s->go_send_data.head[0] = 0xFE;
    GO_motor_s->go_send_data.head[1] = 0xEE;
	
		SATURATE(GO_motor_s->id,   0,    15);
		SATURATE(GO_motor_s->mode, 0,    7);
		SATURATE(GO_motor_s->K_P,  0.0f,   25.599f);
		SATURATE(GO_motor_s->K_W,  0.0f,   25.599f);
		SATURATE(GO_motor_s->T,   -127.99f,  127.99f);
		SATURATE(GO_motor_s->W,   -804.00f,  804.00f);
		SATURATE(GO_motor_s->Pos, -411774.0f,  411774.0f);

    GO_motor_s->go_send_data.mode.id   = GO_motor_s->id;
    GO_motor_s->go_send_data.mode.status  = GO_motor_s->mode;
    GO_motor_s->go_send_data.comd.k_pos  = GO_motor_s->K_P/25.6f*32768;
    GO_motor_s->go_send_data.comd.k_spd  = GO_motor_s->K_W/25.6f*32768;
    GO_motor_s->go_send_data.comd.pos_des  = GO_motor_s->Pos/6.2832f * 32768 * 6.33;
    GO_motor_s->go_send_data.comd.spd_des  = GO_motor_s->W/6.2832f*256 * 6.33 ;
    GO_motor_s->go_send_data.comd.tor_des  = GO_motor_s->T*256;
    GO_motor_s->go_send_data.CRC16 = GO_crc_core(0, (uint8_t *)&GO_motor_s->go_send_data, 15);
	
    return 1;
}

/**
 * @brief 接收数据
 * @param GO_motor_r 接收数据结构体
 * @return 接收数据是否正确（1正确，0不正确）
 */
int GO_recv_data(UNITREE_MOTOR_recv *GO_motor_r)
{
    if(GO_motor_r->go_recv_data.CRC16 !=
        GO_crc_core(0, (uint8_t *)&GO_motor_r->go_recv_data, 14)){
        // printf("[WARNING] Receive data CRC error");
        GO_motor_r->correct = 0;
        return GO_motor_r->correct;
    }
    else
		{
        GO_motor_r->motor_id = GO_motor_r->go_recv_data.mode.id;
        GO_motor_r->mode = GO_motor_r->go_recv_data.mode.status;
        GO_motor_r->Temp = GO_motor_r->go_recv_data.fbk.temp;
        GO_motor_r->MError = GO_motor_r->go_recv_data.fbk.MError;
        GO_motor_r->W = ((float)GO_motor_r->go_recv_data.fbk.speed/256)*6.2832f * 6.33 ;
        GO_motor_r->T = ((float)GO_motor_r->go_recv_data.fbk.torque) / 256;
        GO_motor_r->Pos = 6.2832f*((float)GO_motor_r->go_recv_data.fbk.pos) / 32768/ 6.33 ;//Pos/6.2832f * 32768 / 6.33
				GO_motor_r->go_motor_r.footForce = GO_motor_r->go_recv_data.fbk.force;
				GO_motor_r->correct = 1;
        return GO_motor_r->correct;
    }
}

/***************A1电机控制函数***************/
int A1_send_data(UART_HandleTypeDef *huart,UNITREE_MOTOR_send *A1_motor_s,uint16_t *rxlen)
{ 
    A1_motor_s->hex_len = 34; 
    A1_motor_s->a1_send_data.head.start[0] = 0xFE; 
    A1_motor_s->a1_send_data.head.start[1] = 0xEE; 
    A1_motor_s->a1_send_data.head.motorID = A1_motor_s->id; 
    A1_motor_s->a1_send_data.head.reserved = 0x0; 
    A1_motor_s->a1_send_data.Mdata.mode = A1_motor_s->mode; 
    A1_motor_s->a1_send_data.Mdata.ModifyBit = 0xFF; 
    A1_motor_s->a1_send_data.Mdata.ReadBit = 0x0; 
    A1_motor_s->a1_send_data.Mdata.reserved = 0x0; 
    A1_motor_s->a1_send_data.Mdata.Modify.L = 0; 
    A1_motor_s->a1_send_data.Mdata.T = A1_motor_s->T * 256; 
    A1_motor_s->a1_send_data.Mdata.W = A1_motor_s->W * 128 / 9.1 ; 
    A1_motor_s->a1_send_data.Mdata.Pos = (int)((A1_motor_s->Pos / 6.2832f) * 16384.0f / 9.1); 
    A1_motor_s->a1_send_data.Mdata.K_P = A1_motor_s->K_P * 78.5577f; 
    A1_motor_s->a1_send_data.Mdata.K_W = A1_motor_s->K_W * 102400; 
    A1_motor_s->a1_send_data.Mdata.LowHzMotorCmdIndex = 0; 
    A1_motor_s->a1_send_data.Mdata.LowHzMotorCmdByte = 0; 
    A1_motor_s->a1_send_data.Mdata.Res[0] = A1_motor_s->A1_Res; 
    A1_motor_s->a1_send_data.CRCdata.u32 = A1_crc_core((uint32_t *)(&(A1_motor_s->a1_send_data)), 7); 
    return 0; 
} 
 
int A1_recv_data(UNITREE_MOTOR_recv *A1_motor_r) 
{ 
    /* 对比接收到的数据包CRC 和 计算的CRC32结果 是否一致 */ 
    /* 由于反馈数据包长度为78Byte,而CRC32输入参数为4Byte,78不能被4整除 */ 
    /* 因此反馈数据包的CRC结果前两个字节就不校验了 */ 
    if (A1_motor_r->a1_recv_data.CRCdata.u32 != A1_crc_core((uint32_t *)(&(A1_motor_r->a1_recv_data)), 18)) 
		{
        // Receive data CRC error 
        A1_motor_r->correct = 0; // 标记该包结果不正确 
    } 
    else 
    { 
        A1_motor_r->correct = 1; 
        A1_motor_r->motor_id = A1_motor_r->a1_recv_data.head.motorID; 
        A1_motor_r->mode = A1_motor_r->a1_recv_data.Mdata.mode; 
        A1_motor_r->Temp = A1_motor_r->a1_recv_data.Mdata.Temp; 
        A1_motor_r->MError = A1_motor_r->a1_recv_data.Mdata.MError; 
        A1_motor_r->T = ((float)A1_motor_r->a1_recv_data.Mdata.T) / 256; 
        A1_motor_r->W = ((float)A1_motor_r->a1_recv_data.Mdata.W) / 128 * 9.1; 
        A1_motor_r->a1_motor_r.LW = A1_motor_r->a1_recv_data.Mdata.LW; 
 
        A1_motor_r->a1_motor_r.Acc = (int)A1_motor_r->a1_recv_data.Mdata.Acc; 
        A1_motor_r->Pos = 6.2832f * ((float)A1_motor_r->a1_recv_data.Mdata.Pos) / 16384.0f * 9.1; 
 
        A1_motor_r->a1_motor_r.gyro[0] = ((float)A1_motor_r->a1_recv_data.Mdata.gyro[0]) * 0.00107993176f; 
        A1_motor_r->a1_motor_r.gyro[1] = ((float)A1_motor_r->a1_recv_data.Mdata.gyro[1]) * 0.00107993176f; 
        A1_motor_r->a1_motor_r.gyro[2] = ((float)A1_motor_r->a1_recv_data.Mdata.gyro[2]) * 0.00107993176f; 
 
        A1_motor_r->a1_motor_r.acc[0] = ((float)A1_motor_r->a1_recv_data.Mdata.acc[0]) * 0.0023911132f; 
        A1_motor_r->a1_motor_r.acc[1] = ((float)A1_motor_r->a1_recv_data.Mdata.acc[1]) * 0.0023911132f; 
        A1_motor_r->a1_motor_r.acc[2] = ((float)A1_motor_r->a1_recv_data.Mdata.acc[2]) * 0.0023911132f; 
    } 
    return A1_motor_r->correct; 
} 
 
 
/**
 * @brief 宇树电机串口控制函数
 * @param motor 电机类型
 * @param huart 使用的串口号 
 * @param pData 发送数据结构体
 * @param rData 接收数据结构体
 * @return HAL_StatusTypeDef 
 * @attention 该函数会传回串口的状态，可以用于判断串口是否正常
 *            该函数会阻塞串口，最好在接收中断中使用，或者使用rtos
 */
HAL_StatusTypeDef unitree_uart_Send_recv(motor_type motor ,UART_HandleTypeDef *huart,UNITREE_MOTOR_send *pData, UNITREE_MOTOR_recv *rData)
{
    uint16_t rxlen=0;
    switch(motor)
    {
        case GO_motor:
            GO_send_data(huart,pData,&rxlen);
                 SET_485_DE_UP();
                 SET_485_RE_UP();
              HAL_UART_Transmit(huart, (uint8_t *)pData, sizeof(pData->go_send_data), 10);   
                 SET_485_RE_DOWN();
                 SET_485_DE_DOWN();
             HAL_UARTEx_ReceiveToIdle(huart, (uint8_t *)rData, sizeof(rData->go_recv_data), &rxlen, 10);
            if(rxlen == 0)
                return HAL_TIMEOUT;
        
            if(rxlen != sizeof(rData->go_recv_data))
                return HAL_ERROR;
        
            uint8_t *go_rp = (uint8_t *)&rData->go_recv_data;
            if(go_rp[0] == 0xFD && go_rp[1] == 0xEE)
            {
                rData->correct = 1;
                GO_recv_data(rData);
                return HAL_OK;
            }
            break;
            // return HAL_ERROR;

        case A1_motor:
            A1_send_data(huart,pData,&rxlen); 
            /* 我这里便于演示 使用了阻塞收发 为了获得最佳性能建议您将收发方式改为DMA+IDLE中断方法 */ 
              SET_485_RE_UP();
              SET_485_DE_UP();
              HAL_UART_Transmit(huart, ((uint8_t *)&pData->a1_send_data), sizeof(A1_MotorData_t), 10); 
            
              SET_485_DE_DOWN();
              SET_485_RE_DOWN();
              HAL_UARTEx_ReceiveToIdle(huart, ((uint8_t *)&rData->a1_recv_data), sizeof(A1_ComdDataV3), &rxlen, 10); 
        
            if (rxlen == 0) 
                return HAL_TIMEOUT; 
        
            if (rxlen != sizeof(rData->a1_recv_data)) 
                return HAL_ERROR; 

            uint8_t *a1_rp = (uint8_t *)&rData->a1_recv_data; 
            if (a1_rp[0] == 0xFE && a1_rp[1] == 0xEE) 
            { 
                A1_recv_data(rData); 
                return HAL_OK; 
            } 
            break;
            // return HAL_ERROR; 

        default:
            break;
    }
    return HAL_ERROR;
} 

/**
 * @brief A1电机crc校验
 * @param ptr 接收数据结构体
 * @param len 数据长度
 * @return uint32_t 
 */
uint32_t A1_crc_core(uint32_t *ptr, uint32_t len) 
{ 
    uint32_t xbit = 0; 
    uint32_t data = 0; 
    uint32_t CRC32 = 0xFFFFFFFF; 
    const uint32_t dwPolynomial = 0x04c11db7; 
    for (uint32_t i = 0; i < len; i++) 
    { 
        xbit = 1 << 31; 
        data = ptr[i]; 
        for (uint32_t bits = 0; bits < 32; bits++) 
        { 
            if (CRC32 & 0x80000000) 
            { 
                CRC32 <<= 1; 
                CRC32 ^= dwPolynomial; 
            } 
            else 
                CRC32 <<= 1; 
            if (data & xbit) 
                CRC32 ^= dwPolynomial; 
 
            xbit >>= 1; 
        } 
    } 
    return CRC32; 
} 

float speed=0;
HAL_StatusTypeDef unitree_rc_ctrl(motor_type motor_type, UART_HandleTypeDef *huart,UNITREE_MOTOR_send *p_data, UNITREE_MOTOR_recv *r_data,remoter_t *rc) 
{
    const float DEADZONE = 10.15f;      // 摇杆死区阈值
    const float MAX_ACCELERATION = 0.002f; // 最大加速度值
    const float RC_INPUT_RANGE = 784.0f; // 遥控通道最大输入范围
	
	  const float MAX_POS = Min_Pos + pitch_pos_len;    // 最大位置（需根据实际定义）
    const float MIN_POS = Min_Pos;    // 最小位置

    // 获取遥控输入并计算加速度
    float rc_in = rc->rc.ch[2];
    float Pos_plus = 0;

		const float DECEL_RANGE_RATIO = 0.2f;  // 减速区占全程比例（20%）
    const float total_range = MAX_POS - MIN_POS;  // 总行程范围
    float decel_range = total_range * DECEL_RANGE_RATIO; // 实际减速区长度
		
    // 动态加速度计算
    if(fabs(rc_in) > DEADZONE) {
        // 计算有效输入量
        float effective_input = fabs(rc_in) - DEADZONE;
        float input_ratio = effective_input / (RC_INPUT_RANGE - DEADZONE);
        
        // 基础加速度计算
        float base_accel = input_ratio * MAX_ACCELERATION;
        
        /* 新增减速区逻辑 */
        float current_to_max = MAX_POS - Init_Pos;  // 距上限距离
        float current_to_min = Init_Pos - MIN_POS;  // 距下限距离
        
        // 计算减速系数（1.0表示无减速，0.0表示完全停止）
        float decel_factor = 1.0f;
        
        // 向上运动接近上限
        if(rc_in > 0 && current_to_max < decel_range) {
            decel_factor = current_to_max / decel_range; // 线性衰减
        }
        // 向下运动接近下限
        else if(rc_in < 0 && current_to_min < decel_range) {
            decel_factor = current_to_min / decel_range;
        }
        
        // 应用减速系数（使用平方曲线平滑过渡）
        decel_factor = powf(fmaxf(decel_factor, 0.0f), 2.0f);
        Pos_plus = (rc_in > 0 ? 1.0f : -1.0f) * base_accel * decel_factor;
    }
		Init_Pos += Pos_plus  ;
		SATURATE( Init_Pos , MIN_POS, MAX_POS);
		
    //配置电机控制指令
    p_data->id = pitch_motor_id;
    p_data->mode = go_start;        // FOC模式
		p_data->K_W = 0.05;
		p_data->K_P = 8;
		p_data->Pos = Init_Pos;

    //发送控制指令并接收反馈
    return unitree_uart_Send_recv(motor_type, huart, p_data, r_data);
}

